#include "KalmanState.h"

#include <iostream>
#include "PDController.h"

KalmanState::KalmanState()
{
	current = 0;
	enemy = &World::instance()->enemies()[current];
	outOfRange = false;
}


KalmanState::~KalmanState(void)
{
}

Command KalmanState::GetCommand(Agent* agent){
	otank_t* en = &World::instance()->other_tanks()[current];
	
	if(en->status.compare("dead") == 0 || outOfRange){
		outOfRange = false;
		current ++;
		if(current > World::instance()->other_tanks().size()-1)
			current = 0;
		enemy = &World::instance()->enemies()[current];
		enemy->ResetEx();
	}

	Vector2 aim = enemy->Predict(calcShot(agent->getTank()));//calcShot(agent->getTank());
	Vector2 pos = Vector2(agent->getTank()->pos[0], agent->getTank()->pos[1]);

	if((aim - pos).magnitude() > 350){
		outOfRange = true;
		return Command(agent->getTank()->index, 0, 0, false);
	}

	Command cmd = PDController::instance()->GetComand(agent->getTank(), aim-pos);
	
	//double dtheta = normalizeAngle(aim.angle()) - normalizeAngle(agent->getTank()->angle);
	//std::cout << aim.angle() << " " << normalizeAngle(agent->getTank()->angle) << endl;
	std::cout << cmd.angvel() << endl;
	if(fabs(cmd.angvel()) < .015)//.05)
		cmd.shoot(true);
	
	cmd.angvel(cmd.angvel()*1.7);

	return cmd;
}

/*Vector2 KalmanState::calcShot(tank_t* tank){
	float t = 0.0;
	Vector2 tankP = Vector2(tank->pos[0], tank->pos[1]);
	Vector2 predicted;
	float shotV = World::instance()->getBulletSpeed();
	float shotT = 0.0;

	while(1){
		t += PREDICT_STEP;
		predicted = enemy->Predict(t);
		shotT = (predicted-tankP).magnitude()/shotV;
		if(fabs(shotT-t) < EPSILON || 
		   fabs(predicted.x) * 2.0f > World::instance()->getSize() ||
		   fabs(predicted.y) * 2.0f > World::instance()->getSize())
			return predicted;
	}
}*/

float KalmanState::calcShot(tank_t* tank){
	float t = 0.0;
	Vector2 tankP = Vector2(tank->pos[0], tank->pos[1]);
	Vector2 predicted;
	float shotV = World::instance()->getBulletSpeed();
	float shotT = 0.0;

	while(1){
		t += PREDICT_STEP;
		predicted = enemy->Predict(t);
		shotT = (predicted-tankP).magnitude()/shotV;
		if(fabs(shotT-t) < EPSILON || 
		   fabs(predicted.x) * 2.0f > World::instance()->getSize() ||
		   fabs(predicted.y) * 2.0f > World::instance()->getSize())
			return t;
	}
}


double KalmanState::normalizeAngle(double angle) {
	if (angle > 0) {
		while (angle > 2 * M_PI) {
			angle -= 2 * M_PI;
		}
	} else {
		while (angle < 0) {
			angle += 2 * M_PI;
		}
	}

	return angle;
}